#include "grid_common.h"
#include "serial_communication.h"

/*Flow States
  1: Stable No Person
  2: Lost a person unstable
  3: Has Person stable
  4: Gained a person unstable
*/

static int states[5]; //Keeps track of surround colors and states
static unsigned long stateTimestamps[5];
static boolean flow2StateChanged[5];

long colors[5];
long color = RED;
int state;
boolean changed = true;
boolean isStartEnd = false;
boolean selfChanged = true;

int getMyState() {
  return state;
}

void setMyState(int newState) {
  state = newState;
}

long getMyColor() {
  return color;
}

void setMyColor(long newColor) {
  color = newColor;
}

int getState(int idx) {
  return states[idx];
}

void setState(int idx, int newState) {
  states[idx] = newState;
  stateTimestamps[idx] = millis();
  flow2StateChanged[idx] = true;
}

long getColor(int idx) {
  return colors[idx];
}

void setColor(int idx, long color) {
  colors[idx] = color;
}

boolean isChanged() {
  return changed;
}

void setChanged(boolean newValue) {
  changed = newValue;
}

void flowDebugLight(){  //Gives visual indication of Tile Status via onboard LED
  if(state%2 == 0){
    digitalWrite(13,HIGH); 
  }else{
    digitalWrite(13, LOW);  
  }
}

void initFlow() { //Initializaiton of Flow Mode
  for(int i=0; i<5; i++){
    setState(i, 0);
    colors[i] = 0;
    flow2StateChanged[i] = false;
  }
  color = getStripStates(CIRCLE);
  colors[0] = color;
  if(readSwitch()){
    state = 3;
    //turnOn(CIRCLE, color);
  }else{
    state = 1;
    /*if (!isStartEnd) {
      turnOn(CIRCLE, BLANK);
    }*/
  }
  
  states[0] = state;
  selfChanged = true;
  changed = true;
  setNewState(false);
  Serial.println("Done initing flow");
}

void flow(){ //Original Flow game mode
  flowDebugLight();
  
  if(readSwitch()){ //Basic logic
    if(state == 1){ // We gain a person
      state = 4;
      selfChanged = true;
    } 
    if(state == 2){
      state = 3;
      selfChanged = true;
    }
  }else{
    if(state == 3){ // We lose a person
      state = 2;
      selfChanged = true;
    }
    if(state == 4){
      state = 1;
      selfChanged = true;
    }
  }
  
  if(state == 2 || state == 4){ //If we are unstable...
  /*  if(state == 4 && digitalRead(button) == false){
      Serial.println("Accident Forgiveness 4=>1!");
      selfChanged = true;
      state = 1;
      return; 
    }
    if(state == 2 && digitalRead(button)){ 
      Serial.println("Accident Forgiveness 2=>3!");
      selfChanged = true;
      state = 3;
      return;
    }*/
    for(int i =1; i<5; i++){
      if(( state == 4 && getState(i) == 2) || (state == 2 && getState(i) == 4)){ //Found a match in a neighboring tile
        broadcastStateColor();
        delay(250);
        broadcastStateColor();
        selfChanged = true;
        Serial.println("Connecting to: " + getTileName(i));
        if(state == 4){
          color = colors[i];
          state = 3;
        }else{
          state = 1;  
        }
        if(getStripStates(i) == BLANK){
          turnOn(i,color);
        }else{
          turnOn(i, BLANK);
        }
        
      } 
    }  
  }

  if(changed || selfChanged){ //Prints out states of neighboring tiles
    pushPixels();
    setState(0, state);
    for(int i = 0; i<5; i++){
      //Serial.print(TILE_NAMES[i]); Serial.print(String(states[i]) + " ");
    }
    //Serial.println();
    changed = false;
  }
  
  if(selfChanged){
    broadcastStateColor();
    selfChanged = false; 
  } 
  
  //delay(20);
}

static const unsigned long THRESHHOLD = 4000; // 4 seconds.
unsigned long getTimeDiff(unsigned long time1, unsigned long time2) {
  if (time1 >= time2) {
    return time1 - time2;
  } else {
    return time2 - time1;
  }
}

void clearFlow2State(int portIdx) {
  if (portIdx >= 0 && portIdx < NUMBER_OF_PORTS) {
    flow2StateChanged[portIdx] = false;
  }
}

void sendStateExpirationMsg() {
  int values[] = {STATE_EXPIRATION_MSG_TYPE};
  for (int i=1; i<NUMBER_OF_PORTS; i++) {
    portWrite(i, values, 1);
  }
}

void flow2() {
  // Check whether I gain or lose a person
  boolean myStateUpdated = false;
  if (readSwitch()) {
    if (getState(0) != 3) {
      setState(0, 3);
      myStateUpdated = true;
    }
  } else {
    if (getState(0) != 1) {
      setState(0, 1);
      myStateUpdated = true;
    }
  }
  if (myStateUpdated) {
    state = getState(0);
    color = getColor(0);
    broadcastStateColor();
    // skip the matching to the next iteration, so that we have a chance to receive neighbor's
    // state before matching.
    return;
  }

  unsigned long now = millis();
  for (int i=0; i<NUMBER_OF_PORTS; i++) {
    if (flow2StateChanged[i] && getTimeDiff(now, stateTimestamps[i]) > THRESHHOLD) {
      // Expire old state change
      Serial.println("state change expired for: "); Serial.println(i);
      flow2StateChanged[i] = false;
    }
  }

  if (flow2StateChanged[0]) {
    //Serial.println("finding matches");
    unsigned long minDiff = THRESHHOLD;
    int minPortIdx = -1;
    for (int portIdx =1; portIdx < 5; portIdx++) {
      //Serial.println("checking port: "); Serial.println(portIdx);
      unsigned long diff = getTimeDiff(stateTimestamps[0], stateTimestamps[portIdx]);
      // If the neighbor is in a different state, and it changed recently,
      // most likely we exchange a person
      if ( diff < minDiff && flow2StateChanged[portIdx] && getState(0) != getState(portIdx)) {
        minDiff = diff;
        minPortIdx = portIdx;
      }
    }
    
    if (minPortIdx != -1) {
      if (DEBUG) {
        Serial.println("Matching with neighbor: "); Serial.println(getTileName(minPortIdx));
      }
      flow2StateChanged[0] = false;
      sendStateExpirationMsg();
      flow2StateChanged[minPortIdx] = false;
      // If I have state 3, it means I gain a person. There are two possibilities:
      // 1) it's a new step; 2) it's a re-trace. If it's a new step, I take on neighbor's color.
      if (getState(0) ==3 && getStripStates(minPortIdx) == BLANK) {
        if (DEBUG) {
          Serial.println("taking neighbor's color:");
        }
        colors[0] = colors[minPortIdx];
      }
      if (getStripStates(minPortIdx) == BLANK) {
        turnOn(minPortIdx, colors[0]);
      } else {
        turnOn(minPortIdx, BLANK);
      }
      pushPixels();
    }
  }
}

//**CHANGE IN PROTOCOL**
//Message type 8 now REQUIRES a fith parameter in order to support fast Flow Setup
//Light up multiple tiles at once by merging several messages together. For example 8,0,0,1,0 and 8,1,1,2,0
//can be sent in a single message using 8,0,0,1,0,1,1,2,0.
void setFlowPositions(int* values, int num) {
  if (DEBUG) Serial.println("Setting flow positions");
  for(int idx = 1; idx+3 <num; idx=idx+4){ //Process each part of the message one at a time
    if(values[idx] == getX() && values[idx+1] == getY()){
      if (DEBUG) {
        Serial.println("Recieved message for " + String(values[idx]) + "," + String(values[idx+1]) );
        Serial.println("Thats Me! Direction is " + String(values[idx+3]));
      }

      if(values[idx+3]>=0 && values[idx+3]<=4 && values[idx+2] >=1 && values[idx+2] <= 9){//Check just in case
        turnOn(values[idx+3], numberToColor(values[idx+2]));
        setColor(values[idx+3], numberToColor(values[idx+2]));
      }
      pushPixels();
    }
  }
}

// Preset flow games.
// Format: num of ints, x1, y1, direction, color number, x2, y2, ...
static int flowGame0[] = { // 8x8 8person
  65,
  0,7,1,0,  2,6,1,0,  1,6,2,0,
  4,5,2,0,  1,2,3,0,  5,5,3,0,
  1,1,4,0,  7,6,4,0,  3,6,5,0,
  3,1,5,0,  4,0,6,0,  7,2,6,0,
  5,0,7,0,  7,1,7,0,  7,5,8,0,  7,3,8,0
};
static int flowGame1[] = { // 8x8 7person
  57,
  0,2,1,0,  6,6,1,0,  1,6,2,0,  5,3,2,0,  2,3,3,0,  5,2,3,0,
  2,5,4,0,  6,3,4,0,  3,7,5,0,  5,4,5,0,  4,6,6,0,  7,7,6,0,
  5,6,7,0,  6,4,7,0
};
static int flowGame2[] = { // 7x7 8person
  65,
  0,4,1,0,  2,0,1,0,  0,5,2,0,  5,1,2,0,  0,6,3,0,  4,0,3,0,
  1,2,4,0,  2,1,4,0,  3,0,5,0,  4,1,5,0,  3,5,6,0,  6,4,6,0,
  3,6,7,0,  5,6,7,0,  5,5,8,0,  6,6,8,0
};
static int flowGame3[] = { // 7x7 8person
  65,
  1,0,1,0,  1,6,1,0,  1,5,2,0,  3,6,2,0,  2,2,3,0,  4,3,3,0,
  3,2,4,0,  6,1,4,0,  3,5,5,0,  6,0,5,0,  4,2,6,0,  5,5,6,0,
  4,4,7,0,  4,6,7,0,  5,6,8,0,  6,2,8,0
};
static int flowGame4[] = { // 7x7 7person
  57,
  2,0,1,0,  1,6,1,0,  3,0,2,0,  2,6,2,0,  2,1,3,0,  2,5,3,0,  2,3,4,0,
  5,2,4,0,  4,2,5,0,  3,3,5,0,  3,5,6,0,  5,5,6,0,  2,2,7,0,  5,1,7,0
};
static int flowGame5[] = { // 7x7 7person
  57,
  6,0,1,0,  0,2,1,0,  1,1,2,0,  0,3,2,0,  0,4,3,0,  2,4,3,0,  6,4,4,0,
  0,6,4,0,  6,5,5,0,  4,4,5,0,  5,2,6,0,  4,6,6,0,  6,3,7,0,  6,2,7,0
};
static int flowGame6[] = {
  49,
  2,6,1,0,  5,7,1,0,  2,1,2,0,  3,3,2,0,  1,4,3,0,  1,6,3,0,
  3,6,4,0,  5,2,4,0,  2,0,5,0,  4,1,5,0,  1,1,6,0,  1,3,6,0
};
static int flowGame7[] = {
  49,
  3,3,1,0,  7,3,1,0,  4,1,2,0,  5,6,2,0,  6,1,3,0,  7,2,3,0,
  6,3,4,0,  6,6,4,0,  4,0,5,0,  7,5,5,0,  7,0,6,0,  6,2,6,0
};
static int flowGame8[] = {
  57,
  4,3,1,0,  6,4,1,0,  5,1,2,0,  7,7,2,0,  2,2,3,0,  2,5,3,0,
  3,6,4,0,  7,5,4,0,  6,5,5,0,  7,6,5,0,  5,2,6,0,  6,1,6,0,
  2,0,7,0,  3,5,7,0
};
static int flowGame9[] = {
  65,
  1,1,1,0,  4,3,1,0,  4,0,2,0,  6,2,2,0,  6,0,3,0,  0,7,3,0,
  0,3,4,0,  4,2,4,0,  2,1,5,0,  3,2,5,0,  0,4,6,0,  6,3,6,0,
  1,5,7,0,  5,2,7,0,  1,4,8,0,  4,4,8,0
};

static int* flowGames[] = {
  flowGame0, flowGame1, flowGame2, flowGame3, flowGame4, flowGame5, flowGame6, flowGame7, flowGame8, flowGame9
};

#define NUM_OF_PRESET_GAMES 10

void choosePresetFlowGame(int gameNum) {
  if (gameNum >= 0 && gameNum < NUM_OF_PRESET_GAMES) {
    setFlowPositions(flowGames[gameNum], flowGames[gameNum][0]);
  }
}

void setPresetFlowGame(int* values, int num) {
  if (num < 5)
    return;
  int gameNum = values[0];
  if (gameNum >= 0 && gameNum < NUM_OF_PRESET_GAMES) {
    size_t numOfBytes = sizeof(int) * num;
    int* game = (int *)malloc(numOfBytes);
    game[0] = num;
    memcpy(game+1, values+1, (num-1) * sizeof(int));
    flowGames[gameNum] = game;
  }
}
void appendPresetFlowGame(int* values, int num) {
  if (num < 5)
    return;
  int gameNum = values[0];
  if (gameNum >= 0 && gameNum < NUM_OF_PRESET_GAMES) {
    int currentNum = flowGames[gameNum][0];
    int totalNum = currentNum + num - 1;
    size_t numOfBytes = sizeof(int) * totalNum;
    int* game = (int *)malloc(numOfBytes);
    game[0] = totalNum;
    memcpy(game+1, flowGames[gameNum]+1, (currentNum-1) * sizeof(int));
    memcpy(game+currentNum, values+1, (num-1) * sizeof(int));
    flowGames[gameNum] = game;
  }
}

static int flowShow0[] = { // 5x5
  41,
  2,2,1,0,  5,0,1,0,  2,4,2,0,  5,1,2,0,  3,4,3,0,  5,4,3,0,  4,2,4,0,
  6,4,4,0,  5,2,5,0,  6,0,5,0
};
static int flowShow1[] = { // color 1
  17,
  5,0,1,2,  4,0,1,4,  4,0,1,2,  3,0,1,4
};
static int flowShow2[] = { // color 1
  25,
  3,0,1,2,  2,0,1,4,  2,0,1,3,  2,1,1,1,  2,1,1,3,  2,2,1,1
};
static int flowShow3[] = { // color 2
  49,
  5,1,2,2,  4,1,2,4,  4,1,2,2,  3,1,2,4,  3,1,2,3,  3,2,2,1,  3,2,2,3,  3,3,2,1,  3,3,2,2,  2,3,2,4,  2,3,2,3, 2,4,2,1
};
static int flowShow4[] = { // color 3
  17,
  3,4,3,4,  4,4,3,2,  4,4,3,4,  5,4,3,2
};
static int flowShow5[] = { // color 4
  33,
  4,2,4,3,  4,3,4,1,  4,3,4,4,  5,3,4,2,  5,3,4,4,  6,3,4,2,  6,3,4,3,  6,4,4,1
};
static int flowShow6[] = { // color 5
  21,
  6,0,5,3,  6,1,5,1,  6,1,5,3,  6,2,5,1,  6,2,5,2,  5,2,5,4
};

static const int NUM_OF_STEPS = 7;
static int* flowAnimation[] = {
  flowShow0,
  flowShow1,
  flowShow2,
  flowShow3,
  flowShow4,
  flowShow5,
  flowShow6
};

static int currentStep = 0;
void nextAnimation(int step) {
  if (step >= 0) {
    currentStep = step % NUM_OF_STEPS;
  }
  setFlowPositions(flowAnimation[currentStep], flowAnimation[currentStep][0]);
  currentStep = (currentStep + 1) % NUM_OF_STEPS;
}
